#include "matrix4x4.h"

namespace visualization
{

const double Matrix4x4::eye[] = { 1, 0, 0, 0,
                                  0, 1, 0, 0,
								  0, 0, 1, 0,
								  0, 0, 0, 1 };

const unsigned char Matrix4x4::cofactor_id[][9] = {
	{5, 6, 7, 9, 10, 11, 13, 14, 15},
	{4, 6, 7, 8, 10, 11, 12, 14, 15},
	{4, 5, 7, 8,  9, 11, 12, 13, 15},
	{4, 5, 6, 8,  9, 10, 12, 13, 14},
	{1, 2, 3, 9, 10, 11, 13, 14, 15},
	{0, 2, 3, 8, 10, 11, 12, 14, 15},
	{0, 1, 3, 8,  9, 11, 12, 13, 15},
	{0, 1, 2, 8,  9, 10, 12, 13, 14},
	{1, 2, 3, 5,  6,  7, 13, 14, 15},
	{0, 2, 3, 4,  6,  7, 12, 14, 15},
	{0, 1, 3, 4,  5,  7, 12, 13, 15},
	{0, 1, 2, 4,  5,  6, 12, 13, 14},
	{1, 2, 3, 5,  6,  7,  9, 10, 11},
	{0, 2, 3, 4,  6,  7,  8, 10, 11},
	{0, 1, 3, 4,  5,  7,  8,  9, 11},
	{0, 1, 2, 4,  5,  6,  8,  9, 10}};

const unsigned int Matrix4x4::size = sizeof(double) << 4;

Matrix4x4::Matrix4x4()
{
	LoadIdentity();
}

Matrix4x4::Matrix4x4(const Matrix4x4 &src)
{
	memcpy(element, src.element, size);
}

Matrix4x4& Matrix4x4::operator=(const Matrix4x4 &src)
{
	if (&src == this)
		return *this;

	memcpy(element, src.element, size);
	return *this;
}

Matrix4x4& Matrix4x4::operator+=(const Matrix4x4 &src)
{
	for (int i=0; i<16; i++)
		element[i] += src.element[i];
	return *this;
}

Matrix4x4& Matrix4x4::operator-=(const Matrix4x4 &src)
{
	for (int i=0; i<16; i++)
		element[i] -= src.element[i];
	return *this;
}

double Matrix4x4::Determinant()
{
	double ret = 0, cur_ele = 0;

	for (int i=0; i<4; i++)
	{
		cur_ele = i%2 ? element[i] : -element[i];
		ret += cur_ele * Determinant3x3(
			Cofactor(i, 0),	Cofactor(i, 1), Cofactor(i, 2),
            Cofactor(i, 3), Cofactor(i, 4), Cofactor(i, 5),
            Cofactor(i, 6), Cofactor(i, 7), Cofactor(i, 8));
	}

    return ret;
}

void Matrix4x4::LoadIdentity()
{
	memcpy(element, eye, size);
}

void Matrix4x4::Transpose()
{
	for (int i=0; i<4; i++)
		for(int j=i; j<4; j++)
			std::swap(element[Index(i, j)], element[Index(j, i)]);
}

void Matrix4x4::Adjoint()
{
	Matrix4x4 temp;
	double cur_ele;

	for (int i=0; i<16; i++)
	{
		cur_ele = i%2 ? element[i] : -element[i];
		temp.element[i] = cur_ele * Determinant3x3(
			Cofactor(i, 0),	Cofactor(i, 1), Cofactor(i, 2),
            Cofactor(i, 3), Cofactor(i, 4), Cofactor(i, 5),
            Cofactor(i, 6), Cofactor(i, 7), Cofactor(i, 8));
	}

	*this = temp;
}

bool Matrix4x4::Invert()
{
	double det = Determinant();

	if (EQZero(det))
		return false;

	Adjoint();

	for (unsigned int i=0; i<size; i++)
		element[i] /= det;

	return true;
}

void Matrix4x4::Zero()
{
	memset(element, 0, size);
}

const Matrix4x4 operator+(const Matrix4x4 &lhs, const Matrix4x4 &rhs)
{
	Matrix4x4 ret(lhs);
	ret += rhs;
	return ret;
}

const Matrix4x4 operator-(const Matrix4x4 &lhs, const Matrix4x4 &rhs)
{
	Matrix4x4 ret(lhs);
	ret -= rhs;
	return ret;
}

}
